﻿/*****************************************************************************
** Includes
*****************************************************************************/

#include "RobotCmdNode.hpp"
#include "SerialPinch.h"

/*****************************************************************************
** Namespaces
*****************************************************************************/
using namespace std::chrono_literals;
using std::placeholders::_1;

/*****************************************************************************
** Implementation
*****************************************************************************/

RobotCmdNode::RobotCmdNode(SerialPinch* sp)
    : Node("serialPinch_node", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)), m_sp(sp)
{
    // m_pubCmd = this->create_publisher<pccmd_msg::msg::PcCmd>("PcCmdMsg",1);

    subscription_ = this->create_subscription<pccmd_msg::msg::PcCmd>(
        "PcCmdMsg", 10, std::bind(&RobotCmdNode::topic_callback, this, _1));

    getParameters();
    m_sp->setCom(m_strComName, m_nMaxI, m_nCmderType);
}

RobotCmdNode::~RobotCmdNode()
{
    rclcpp::shutdown();
}

void RobotCmdNode::topic_callback(const pccmd_msg::msg::PcCmd::SharedPtr msg) const
{
    if (msg->cmdname != "pinch") {
        return;
    }

    RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->cmdname.c_str());

    if (msg->cmdcontent == "spin45") {
        m_sp->spin45();
    } else if (msg->cmdcontent == "180x9") {
        m_sp->suojin180x9();
    } else if (msg->cmdcontent == "songkai") {
        m_sp->songkai();
    } else if (msg->cmdcontent == "spinBack45") {
        m_sp->callSpinBack45();
    } else if (msg->cmdcontent == "stop") {
        m_sp->callStop();
    } else if (msg->cmdcontent == "resetPos") {
        m_sp->callResetPos();
    }
}

void RobotCmdNode::getParameters()
{
    RCLCPP_INFO(this->get_logger(), "getParameters...");

    get_parameter_or(std::string("com_name"), m_strComName, std::string("no"));

    get_parameter_or(std::string("max_i"), m_nMaxI, 200);

    get_parameter_or(std::string("cmder_type"), m_nCmderType, 2);

    RCLCPP_INFO(this->get_logger(), "Device Serial No: %s, max_i: %d, cmder_type: %d", m_strComName.c_str(), m_nMaxI, m_nCmderType);
}
